#pragma once
#include <opencv2/opencv.hpp>
#include <iostream>
#include <algorithm>
#include "common.h"
#define    MIN_RATE       CV_PI/6                           // 每秒30度
#define    MIN_SPEED      0.2                               // 每秒平移0.2m
#define    MINI_BATCH     50
#define    RADIUS         0.2
#define    FPS            60
using namespace cv;
using namespace std;

namespace Horizon{

class AntiGyro{
public:
    AntiGyro():is_gyro(false),is_first(true){
        count = 0;
        i = 0;
        is_first = true;
        //palstance = 0;
        armors.clear();
        is_gyro = false;
        direction = 0;
    };
    //两个接口，执行初始化，在发生切换装甲板的时候使用
    void initData(){
        count = 0;
        i = 0;
        is_first = true;
        //palstance = 0;
        armors.clear();
        is_gyro;
        direction = 0;
    }
    
public:
    bool generalFramework(Armor &armor,GimbalPose &gm,CameraMode &cameramode,GimbalPose &return_gm,float v0,float &frequence,float start_time);                          // 总框架

public:
    void getArmor(Armor &armor);                            // 获取装甲板的信息，连续几帧
    void getParam(GimbalPose &gm,CameraMode &cameramode,float v0);   // 向外获取参数

public:
    bool is_Gyro();                                         // 判断是否是陀螺，单双目两种，方法也可以尝试两种
    GimbalPose hitGyro();                                   // 打击方法可以多做尝试

private:
    float               start_time_;                        // 开始发弹的时间
    float                frequence_;                        // 频率
    float                        v0;                        // 子弹初速度
    vector<Point3f>     track_point;                        // 移动陀螺跟踪点,y坐标为装甲板中心点坐标
    Speed                     w_rad;                        // 角速度
    bool                   is_first;                        // 是否第一次进入                     
    vector<Armor>            armors;
    CameraMode           cameramode;
    GimbalPose    return_gimbalpose;                        // 要返回的云台位姿
    GimbalPose       cur_gimbalpose;                        // 现在云台姿态
    Point3f                 center_;                        // 机器人的旋转中心
    bool                    is_gyro;                        // 是否是陀螺
    int                   direction;                        // 判断旋转方向
    bool                       flag;                        // 启动判断的标志位，true为集齐60帧，false为is_gyro为真
    /*计数变量*/
    int count;                                              // 陀螺为真的计数变量
    int i;                                                  /*可能出问题的点*/

private:
    double getAngle(Armor &armor);                          // 获取角度   
    bool judgeIsRelevant();                                 // 判断数据是否有相关性
    Speed getPalstance();                                   // 得到判断标准，角速度，最小二乘法
    void getCenter();                                       // 打击运动陀螺要跟随的点
    
    //template <class T>
    void smoothDataFilter(vector<Armor> &list);             // 平滑数据用的，防止断层
    Armor smoothDataFilter1(vector<Armor> &list);
    float average(vector<double> &list);                    // 求平均值
    void swap(double *a, double *b){                        //交换数据
        double temp;
        temp = *a;
        *a = *b;
        *b = temp;
    }
    bool isMoveGyro();
    float bullteFlyTime(Armor &armor_fix);
};

}